#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include "Classifier.h"
#include "PathMap.h"
#include "Node.h"
#include "Astar.h"
#include "Tree.h"
#include <iostream>
#include <fstream>
#include <drawstuff/drawstuff.h>
#include <string>
#include <cstdlib>
#include <ctime>
#include <utility>
#include <iomanip>
#include <stack>
#include <stdio.h>
#include <time.h>
#include <sys/resource.h>

#define GOALX 2.75
#define GOALY 0.00
#define d 0.01

using namespace std;

// get the elapsed time. This won't overflow, like clock().
double getcputime(void)        
{ struct timeval tim;        
  struct rusage ru;        
  getrusage(RUSAGE_SELF, &ru);        
  tim = ru.ru_utime;        
  double t=(double)tim.tv_sec + (double)tim.tv_usec / 1000000.0;        
  tim=ru.ru_stime;        
  t+=(double)tim.tv_sec + (double)tim.tv_usec / 1000000.0;        
  return t; 
}

void printPathToFile(Node* finish, const char* filename){
     stack<Node*> path;
 
     // retrace the path
     while(finish->parent != NULL){ //while finish isn't the start node
         path.push(finish); //put finish on the stack
         finish = finish->parent; //finish = its parent
     }
     

     // print the file of actions
     FILE * pathFile;
     pathFile = fopen(filename, "w");
     printf("Printing path file \n");
     if(!pathFile){
        printf("Error opening the file!\n");
     }


    // loop along the path
     while(!path.empty()){
        Node* nextStep = path.top();
        path.pop();
        //print the step at this node
        fwrite(&nextStep->moved_foot, sizeof(int), 1, pathFile);
        fwrite(&nextStep->moved_foot_x, sizeof(float), 1, pathFile);
        fwrite(&nextStep->moved_foot_y, sizeof(float), 1, pathFile);
     }

     //write an integer that signifies the end of file
     int ender = -1;
     fwrite(&ender, sizeof(int), 1, pathFile);
     fclose(pathFile);

}



/**
 * Here is where all the magic happens.  With the 
 * default setup, the -path flag will load "actions.path"
 * and attempt to simulate the footsteps line by line.
 * This is for us to see the robot walk along your path.
 * We have provided an example file to make the robot walk
 * .2 meters ahead.  Note that the simulation starts off
 * paused.  You can use Ctrl+P to move it ahead one step at
 * a time, or you can change the "pause" argument in 
 * simulator/LittleDog.cpp to be something else.  To see
 * the terrain press 't' (note the terrain is simulated
 * even if it is not displayed graphically).
 * 
 * Your job is to write the code that will generate
 * the path file - that is you must discretize the search
 * space, perform A* search using a logistic regression
 * classifier (trained before hand or on the fly), and
 * finally write the returned path to file.  Once the 
 * milestone is submitted you may change virtually anything
 * you want to improve performance, including the controller
 * used.  When you submit, you will tell us the flags your
 * program needs to be run with in order to get the desired
 * behavior.
 */

bool runPath(string file, SDSimulator *simulator, SDLittleDog& littleDog,
	     SDSlowWalk& walkController, int runFrames){

    //open paths file
  FILE * paths;
  paths = fopen(file.c_str(), "r");
  if(paths == NULL){
      printf("error opening the file!\n");
      return false;
  }

  int steps = 1;
  //if runFrames = 0, dont display the simulation
  bool skip = runFrames == 0;
  if(skip) runFrames = 1000000;

  SDSimulatorState state;
  SDSimulatorState oldState;
  SDSimulatorState olderState;
  int oldFoot;
  float oldX;
  float oldY;
  int foot = 0;
  float x; 
  float y;
  void* line = new int[1];


  while(foot != -1){

    //read the move from the file
    fread(line, sizeof(int), 1, paths);
    foot = *(int*)(line);
    fread(line, sizeof(float), 1, paths);
    x = *(float*)(line);
    fread(line, sizeof(float), 1, paths);
    y = *(float*)(line);

    //if its at the end of the file
    if(foot == -1)
        break;

    oldFoot = foot;
    oldX = x;
    oldY = y;

    //check whether to display the simulation
    int stepsDis = (steps - 1)%runFrames;
    if(stepsDis == 0 && !skip) littleDog.setSimUI(true);

    olderState = oldState;
    oldState = state; 

    //move the dog   
    simulator->getState(&state);
    walkController.setStepPosition(foot, x, y);
    littleDog.runTrial(&state);

    bduVec3f globalFeet[4];
    simulator->getGlobalFootPositions(globalFeet);
    simulator->getState(&state);
    simulator->setState(&state);  // This prevents the simulator straying from search - see handout faq (last page) for details

    //This part checks to see if dog flipped
    float zoff = 0;
    for(int z=0; z<4; z++){ //this loop checks if the dog has flipped
        zoff += fabs(globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1]));
    }
    if( zoff > 10 ){
        printf("\nOh my dog it flipped!  on step #%d:\nfoot: %d, x: %f,y: %f\n", steps - 1, oldFoot, oldX, oldY);
        getchar();
        littleDog.setSimUI(true);
        simulator->setState(&olderState);
        walkController.setStepPosition(oldFoot, oldX, oldY);
        littleDog.runTrial(&olderState);
        
    }

    if(stepsDis == 0) littleDog.setSimUI(false);
    steps++;
  }

  fclose(paths);
  return true;
}

//This method runs the program - Commented throughout
int main(int argc, char** argv){

  // Initialization
  SDSimulator *simulator;
  SDLittleDog littleDog;
  SDSlowWalk walkController;
  littleDog.setSimulated(true);
  littleDog.setController(&walkController);
  littleDog.setSimUI(false); //set it true if you want to see the simulation
  walkController.setLittleDog(&littleDog);
  littleDog.initializeSimulator();
  simulator = littleDog.getSimulator();
  int allottedTime;

  //if the argument is -path x, it will display the dog every x steps
  //except if the argument is -path 0, in which it will never display the dog
  //if the arugment is -timelimit x, then it will terminate the search 
  //and output a path after x minutes
  if(argc > 1 && !strcmp(argv[1],"-path")){
    int runFrames; //will display dog graphically every this many steps
    if(argc > 2) runFrames = atoi(argv[2]);
    else runFrames = 1;
    runPath("actions.path", simulator, littleDog, walkController, runFrames);
    return 0;
  }else if(argc > 1 && !strcmp(argv[1],"-timelimit")){
      if(argc == 2){
          printf("ERROR: enter how many minutes you want for the time limit\n");
          return 0;
      }else{
         allottedTime = atoi(argv[2]);
         printf("allotted time : %d minutes = %d seconds\n", allottedTime, allottedTime*60); 
         allottedTime = allottedTime*60;
      }

  }

//Make the pathmap!
//just a note - all 4 last values always MUST be around 0, 3, -1.5, and 1.5, 
//because they are the bounds of the terrain
  printf("Making a pathmap.... \n");
  PathMap* bod  = new PathMap(simulator, .01, -.20, 3, -1.5, 1.5);
  printf("Pathmap done. \n");
  bod->printGradMapToFile("gradmap.txt");
  bod->printHeightMapToFile("heightmap.txt");

// search for a 2D path using the pathmap
  vector<Point*> myPath;
  char filename[50];
  printf("Searching for path\n");
  myPath = bod->findPath(); 
  int pathSize = (int)myPath.size();
  printf("path is found with length = %d \n", pathSize);
  bod->printPathToFile("path.txt", myPath);

////////////////////////////////////////////
/////////////A* mover
/////////////////////////////////////////////

  // Create start node
     bduVec3f* globalFeet = new bduVec3f[4];
     simulator->getGlobalFootPositions(globalFeet);
     Node* nstart = new Node();
     nstart->foot_pos = globalFeet;
     nstart->moving_foot = 0;
     nstart->g = 0;
     nstart->center = FindNodeCenter(nstart);
     nstart->h = 10; //easy for now
     nstart->f = nstart->g + nstart->h;
     nstart->parent = NULL;
     nstart->zoff = 0;
     SDSimulatorState startState;
     simulator->getState(&startState);
     nstart->state = &startState;
     Node* current = nstart;
     int i = myPath.size()-1;//step along the 2D path
     int pathTries = 1;//number of times we tried
     double usedTime = 0;
     float closestDist = 2.75;
     float nodesGoalDist;


// run little Astar searches along the 2D path
//If the search fails, add an obstacle around the difficult point, find the new 2D path and start over.
// Note : the 2D path points are numbered starting from the goal point.
    while(i>=0){
         printf("\n");
         usedTime = getcputime();
         printf("Time used : %f seconds\n", usedTime);

         //if this path is the closest one to the node yet, save it to file
         nodesGoalDist = sqrt( (GOALX-current->center[0])*(GOALX-current->center[0]) + (GOALY-current->center[1])*(GOALY-current->center[1]) );
         if( nodesGoalDist < closestDist ){
             printf("Total 2D path tries : %d", pathTries);
             closestDist = nodesGoalDist;
             printPathToFile(current, "actions.path");
         }

         printf("setting goal #%d/%d : %f %f\nPath tries : %d\n", i, myPath.size(),myPath[i]->x ,myPath[i]->y, pathTries);
         Astar* this_search = new Astar(myPath[i]->x ,myPath[i]->y , simulator, &walkController, &littleDog, 0.01, current);
         current = this_search->runSearch();

         // if we get stuck at the ith point on the 2D path
         if(current == NULL){
            printf("Failed at point #%d along the path.\nTrying a new path search...\n",i);

            // add an obstacle around the difficult zone
            bod->addObstacle(myPath[i]);
            // search for a new path
            pathTries++;
            printf("Starting over, this is the %dth try\n", pathTries);
            myPath = bod->findPath(); 
            pathSize = (int)myPath.size();
            printf("high level path is found with length = %d \n", pathSize);
            sprintf (filename, "path%d.txt", pathTries);
            bod->printPathToFile(filename, myPath);
            // relaunch the search at the start Node, with the new path.
            i = myPath.size();
            current = nstart;
         }
       i--;
    }

    printPathToFile(current, "actions.path");
    printf("Total 2D path tries : %d\n", pathTries);

    //timing
    printf("total time : %f seconds\n" ,getcputime());

  return 0;
}
